#include <iostream>
#include <Eigen/Dense>
#include <cmath>

int main() {
    // 输入旋转矩阵
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix << 0.9505638, -0.2940438,  0.0998334,
                    0.3050683,  0.8242422, -0.4770304,
                    0.0579809,  0.4839039,  0.8731983;

    // 输入欧拉角（单位：度）
    double rotate_euler_x = 30.0; // X轴旋转角度
    double rotate_euler_y = 45.0; // Y轴旋转角度
    double rotate_euler_z = 60.0; // Z轴旋转角度

    // 方法一
    Eigen::Vector3d x_axis = rotation_matrix.col(0);
    Eigen::Matrix3d RX = Eigen::AngleAxisd((rotate_euler_x * M_PI / 180.0), x_axis).toRotationMatrix();
    Eigen::Matrix3d R1 = RX * rotation_matrix;

    Eigen::Vector3d y_axis = R1.col(1);
    Eigen::Matrix3d RY = Eigen::AngleAxisd((rotate_euler_y * M_PI / 180.0), y_axis).toRotationMatrix();
    Eigen::Matrix3d R2 = RY * R1;

    Eigen::Vector3d z_axis = R2.col(2);
    Eigen::Matrix3d RZ = Eigen::AngleAxisd((rotate_euler_z * M_PI / 180.0), z_axis).toRotationMatrix();
    Eigen::Matrix3d R3_method1 = RZ * R2;

    // 输出方法一的结果
    std::cout << "Method 1 Result (R3):\n" << R3_method1 << std::endl;



    // 方法二
    Eigen::Matrix3d RX2 = Eigen::AngleAxisd((rotate_euler_x * M_PI / 180.0), rotation_matrix.col(0)).toRotationMatrix();
    Eigen::Matrix3d RY2 = Eigen::AngleAxisd((rotate_euler_y * M_PI / 180.0), rotation_matrix.col(1)).toRotationMatrix();
    Eigen::Matrix3d RZ2 = Eigen::AngleAxisd((rotate_euler_z * M_PI / 180.0), rotation_matrix.col(2)).toRotationMatrix();

    Eigen::Matrix3d R3_method2 = (RX2 * RY2 * RZ2) * rotation_matrix;


    // 输出方法二的结果
    std::cout << "Method 2 Result (R3):\n" << R3_method2 << std::endl;


    // 方法三
    Eigen::Matrix3d RX3 = Eigen::AngleAxisd((rotate_euler_x * M_PI / 180.0), Eigen::Vector3d::UnitX()).toRotationMatrix();
    Eigen::Matrix3d RY3 = Eigen::AngleAxisd((rotate_euler_y * M_PI / 180.0), Eigen::Vector3d::UnitY()).toRotationMatrix();
    Eigen::Matrix3d RZ3 = Eigen::AngleAxisd((rotate_euler_z * M_PI / 180.0), Eigen::Vector3d::UnitZ()).toRotationMatrix();

    Eigen::Matrix3d R3_method3 = rotation_matrix * RX3 * RY3 * RZ3;

    // 输出方法三的结果
    std::cout << "Method 3 Result (R3):\n" << R3_method3 << std::endl;

    return 0;
}
